/* FreeRTOS includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
// 引入 FreeRTOS 信号量头文件
#include "semphr.h" 

/* Xilinx includes. */
#include "xil_printf.h"
#include "xstatus.h" // 包含 XST_SUCCESS 等状态码
#include "xil_cache.h" // 包含 Xil_DCacheFlushRange 的声明

// 引入封装好的 VDMA 显示模块 (C头文件)
#include "vdma_display.h"
// 引入 HLS GPU_Render_TBR IP 的驱动
#include "xgpu_render_tbr.h"

#include <string.h>  // for memset(), memcpy()
#include <stdlib.h>  // for malloc, free if needed later, and also for `strtol` for number parsing if applicable
#include <stdio.h>   // 用于 sprintf 浮点格式化

#include "font.h"    // 用于字体绘制

// --- 全局实例，以便任务函数可以访问 ---
VdmaDisplay g_vdma_display;

// --- 全局配置常量---
#define GPU_RENDER_TBR_BASE_ADDR    XPAR_GPU_RENDER_TBR_0_BASEADDR // HLS IP 的 AXI-Lite 控制接口地址

// --- GPU IP 使用的 DDR 缓冲区地址 ---
// 参数缓冲区：紧接在所有 HLS 和 VDMA 帧缓冲区之后
#define GPU_PARAMS_BUFFER_BASE_ADDR (VDMA_FRAME_BUFFER_BASE_ADDR + ((UINTPTR)VDMA_SINGLE_FRAME_SIZE * VDMA_NUM_FRAME_BUFFERS))

// 定义一个辅助函数进行向上对齐
#define align_up(addr, align_bytes) (((addr) + (align_bytes) - 1) & ~((align_bytes) - 1))

// --- HLS IP 常量 (来自 gpu_render.hpp, pbirsm.hpp, hr_tbr.hpp 等) ---
// 需要确保这些与 HLS IP 设计完全匹配
#define MAX_NUM_TRIANGLES           1023    // from gpu_render.hpp

// HLS IP 内部的 DDR 字宽类型
typedef u64 ddr_word_t; // 对应 ap_uint<64>

#define NUM_AXI_WORDS_PER_PACKET    23

// GPU_PARAMS_TOTAL_BYTE_SIZE 用于估算 params_buffer 的最大大小
#define GPU_PARAMS_TOTAL_BYTE_SIZE (MAX_NUM_TRIANGLES * NUM_AXI_WORDS_PER_PACKET * sizeof(ddr_word_t))

// ZBuffer：紧接在参数缓冲区之后，并考虑 128 字节对齐
// ZBuffer 是 u16 类型的，大小为 width * height * 2 字节。
#define GPU_ZBUFFER_BASE_ADDR align_up(GPU_PARAMS_BUFFER_BASE_ADDR + GPU_PARAMS_TOTAL_BYTE_SIZE, 128)


// --- GPU 驱动实例 ---
static XGpu_render_tbr GpuRenderTbrInstance;


// =========================================================================================
// 手动实现的数学函数
// =========================================================================================
/**
 * @brief 手动实现 fabsf 功能：返回浮点数的绝对值。
 * @param x 输入浮点数
 * @return x 的绝对值 (float 类型)
 */
static inline float my_fabsf(float x) {
    // 检查符号位，如果是负数则取反，否则直接返回
    return (x < 0.0f) ? -x : x;
}


/**
 * @brief 手动实现 floorf 功能：返回小于或等于 x 的最大整数值 (float 类型)。
 * @param x 输入浮点数
 * @return 小于或等于 x 的最大整数 (float 类型)
 */
static inline float my_floorf(float x) {
    long l_x = (long)x;
    // 如果 x 是负数且有小数部分，则需要向下取整到更小的整数
    // 例如：-3.5 的 floor 是 -4，但 (long)-3.5 是 -3。
    // 所以当 x < 0 且 (float)l_x != x 时，l_x 需要减 1。
    if (x < 0.0f && (float)l_x != x) {
        return (float)(l_x - 1);
    }
    return (float)l_x;
}

/**
 * @brief 手动实现 roundf 功能：返回四舍五入到最接近的整数值 (float 类型)，
 *        遵循 "round half to even" (也称为银行家舍入法或最近偶数舍入)。
 *        对于 x.5 的情况，向最近的偶数取整。例如：2.5 -> 2, 3.5 -> 4。
 * @param x 输入浮点数
 * @return 四舍五入后的整数 (float 类型)
 */
static inline float my_roundf(float x) {
    // 检查是否为负数，并暂时按正数处理，最后再反转符号
    int sign = (x < 0.0f) ? -1 : 1;
    x = my_fabsf(x);

    float fractional_part = x - my_floorf(x); // 获取小数部分

    if (fractional_part > 0.5f) {
        return (my_floorf(x) + 1.0f) * sign;
    } else if (fractional_part < 0.5f) {
        return my_floorf(x) * sign;
    } else { // fractional_part == 0.5f (exactly half)
        // Round half to even
        float integral_part = my_floorf(x);
        if (((long)integral_part % 2) == 0) { // 如果整数部分是偶数，则向下取整
            return integral_part * sign;
        } else { // 如果整数部分是奇数，则向上取整
            return (integral_part + 1.0f) * sign;
        }
    }
}

// =========================================================================================
// PS端数据类型定义和转换函数 (模拟 HLS ap_fixed/ap_uint 的行为)
// =========================================================================================

// ==================== fp_coord_t (ap_fixed<31, 21, AP_RND_CONV, AP_SAT>) ====================
// HLS定义：typedef ap_fixed<31, 21, AP_RND_CONV, AP_SAT> fp_coord_t;
// W=31, I=21, F=10 (W-I)
const int PS_FP_COORD_W = 31; // Total bits
const int PS_FP_COORD_I = 21; // Integer bits (including sign)
const int PS_FP_COORD_F = (PS_FP_COORD_W - PS_FP_COORD_I); // Fractional bits = 10
const float PS_FP_COORD_SCALE_FACTOR = 1024.0f; // powf(2.0f, PS_FP_COORD_F); // 2^10 = 1024.0f

// HLS ap_fixed<W,I> 内部存储为 W 位的有符号整数。
// 对于 31 位，其范围是 [-2^30, 2^30 - 1]。
const s64 PS_FP_COORD_MAX_INTERNAL_VAL = (1LL << (PS_FP_COORD_W - 1)) - 1;   // max positive: 1073741823
const s64 PS_FP_COORD_MIN_INTERNAL_VAL = -(1LL << (PS_FP_COORD_W - 1));     // min negative: -1073741824

/**
 * @brief 将 PS 端浮点数 (float) 转换为 HLS IP核期望的 31 位定点数内部整数表示 (s32)。
 *        模拟 ap_fixed 的舍入 (AP_RND_CONV, round half up/even) 和饱和 (AP_SAT) 行为。
 *        这个内部整数值会被写入 DDR/AXI-Lite。
 * @param val 输入浮点数
 * @return 转换为 31 位定点数的内部整数表示 (s32)。
 */
 static inline s32 ps_float_to_fp_coord_internal(float val) {
    // 1. Scale: 乘以 2^F
    float scaled_val = val * PS_FP_COORD_SCALE_FACTOR;

    // 2. Rounding: AP_RND_CONV 是 round half to even (IEEE 754 standard rounding)
    long long internal_val_before_sat = (long long)my_roundf(scaled_val);

    // 3. Saturation: 确保值在 31 位有符号整数的范围内
    s64 internal_val = internal_val_before_sat; // 使用 s64 进行饱和检查
    if (internal_val > PS_FP_COORD_MAX_INTERNAL_VAL) internal_val = PS_FP_COORD_MAX_INTERNAL_VAL;
    if (internal_val < PS_FP_COORD_MIN_INTERNAL_VAL) internal_val = PS_FP_COORD_MIN_INTERNAL_VAL;

    return (s32)internal_val; // 返回 31 位有符号整数
 }


// ==================== fp_z_buffer_t (ap_uint<16>) ====================
// HLS定义：typedef ap_uint<16> fp_z_buffer_t;
/**
 * @brief 将 PS 端浮点数 (例如归一化深度) 转换为 16 位无符号 Z Buffer 值 (0-65535)。
 *        模拟 HLS ap_uint<16> 的截断行为。
 * @param z_float 输入浮点数
 * @return 转换后的 16 位无符号 Z Buffer 值 (u16)
 */
 static inline u16 ps_float_to_fp_z_buffer(float z_float) {
    // 钳制到 [0, 65535] 范围
    float clamped_val = z_float;
    if (clamped_val < 0.0f) clamped_val = 0.0f;
    if (clamped_val > 65535.0f) clamped_val = 65535.0f;

    u16 result = (u16)my_floorf(clamped_val);
    return result;
 }


// ==================== fp_color_channel_t (ap_uint<8>) ====================
// HLS定义：typedef ap_uint<8> fp_color_channel_t;
/**
 * @brief 将 PS 端浮点数或整数转换为 8 位无符号颜色通道值 (0-255)。
 *        模拟 HLS ap_uint<8> 的截断行为。
 * @param color_val 输入数值
 * @return 转换后的 8 位无符号颜色通道值 (u8)
 */
 static inline u8 ps_to_fp_color_channel(float color_val) {
    // 钳制到 [0, 255] 范围
    float clamped_val = color_val;
    if (clamped_val < 0.0f) clamped_val = 0.0f;
    if (clamped_val > 255.0f) clamped_val = 255.0f;

    u8 result = (u8)my_floorf(clamped_val);
    return result;
 }


// =========================================================================================
// DDR 数据打包/解包函数 (处理 64 位 ddr_word_t)
// =========================================================================================

/**
 * @brief 将 R, G, B 分量打包成 32 位颜色槽 (低 24 位是 BGR)。
 *        用于 PS 端初始化 Frame Buffer 或验证读取。
 *        与 HLS 端打包逻辑保持一致：B (bits 0-7), G (bits 8-15), R (bits 16-23)。
 *        高 8 位为 0 (0x00RRGGBB)。
 * @param r, g, b 颜色分量 (0-255)
 * @return 打包后的 32 位颜色槽值 (u32)，高 8 位为 0。
 */
 static inline u32 ps_pack_rgb_to_32bit_slot(u8 r, u8 g, u8 b) {
    u32 packed_color = 0;
    packed_color |= (u32)b << 0;  // B (0-7)
    packed_color |= (u32)g << 8;  // G (8-15)
    packed_color |= (u32)r << 16; // R (16-23)
    return packed_color; // 高 8 位是 0
 }

// --- PS 端模拟几何处理所需的数据结构 ---
// 对应 HLS IP 内部使用的 `raster_params_packet_fixed` 结构
typedef struct {
    float v0x, v0y, v0z_norm; // 归一化Z值 [0, 1]
    float v1x, v1y, v1z_norm;
    float v2x, v2y, v2z_norm;
    u8 r, g, b; // 纯色
    // Edge equation coefficients
    float edge0_A, edge0_B, edge0_C;
    float edge1_A, edge1_B, edge1_C;
    float edge2_A, edge2_B, edge2_C;
    // Depth gradients
    float dzdx, dzdy;
} PSTriangle;


// 辅助函数：计算三角形的边缘方程系数 (Ax + By + C = 0)
void calculate_edge_equations_float(
    float v0x, float v0y, float v1x, float v1y, float v2x, float v2y,
    float* A0, float* B0, float* C0,
    float* A1, float* B1, float* C1,
    float* A2, float* B2, float* C2
) {
    *A0 = v0y - v1y;
    *B0 = v1x - v0x;
    *C0 = v0x * v1y - v1x * v0y;

    *A1 = v1y - v2y;
    *B1 = v2x - v1x;
    *C1 = v1x * v2y - v2x * v1y;

    *A2 = v2y - v0y;
    *B2 = v0x - v2x;
    *C2 = v2x * v0y - v0x * v2y;
}

// 辅助函数：计算深度梯度 (dzdx, dzdy)
void calculate_depth_gradients_float(
    float v0x, float v0y, float v0z, // v0z is 1/Z (normalized 0..1)
    float v1x, float v1y, float v1z,
    float v2x, float v2y, float v2z,
    float* dzdx, float* dzdy
) {
    float dx1 = v1x - v0x;
    float dy1 = v1y - v0y;
    float dz1 = v1z - v0z;

    float dx2 = v2x - v0x;
    float dy2 = v2y - v0y;
    float dz2 = v2z - v0z;

    float det = dx1 * dy2 - dx2 * dy1;

    if (my_fabsf(det) < 1e-6f) { // 防止除以零
        *dzdx = 0.0f;
        *dzdy = 0.0f;
        return;
    }

    float inv_det = 1.0f / det;
    *dzdx = (dz1 * dy2 - dz2 * dy1) * inv_det;
    *dzdy = (dx1 * dz2 - dx2 * dy1) * inv_det;
}

// 定义一个足够大的缓冲区来格式化浮点数
#define FLOAT_STR_BUFFER_SIZE 32

// 辅助函数：将 PSTriangle 打包成 HLS IP 期望的 64 位 DDR 字
void pack_raster_params_to_ddr(const PSTriangle* tri_ps, ddr_word_t* output_ddr_words, u32 triangle_idx) {
    u32 current_packet_word_offset = triangle_idx * NUM_AXI_WORDS_PER_PACKET;
    u32 word_idx = 0; // 当前数据包内的字索引

    s32 internal_val_coord; // 31位定点坐标的内部整数表示
    u16 internal_val_z;     // 16位ZBuffer值
    u8 internal_val_color;  // 8位颜色通道值
    ddr_word_t ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->v0x);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // V0_y
    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->v0y);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // V1_x
    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->v1x);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // V1_y
    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->v1y);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // V2_x
    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->v2x);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // V2_y
    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->v2y);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // V0_z, V1_z, V2_z (注意 Z 值转换：1.0f - Z_norm 才能符合 HLS IP 的 "数值越小越近" 逻辑)
    internal_val_z = ps_float_to_fp_z_buffer((1.0f - tri_ps->v0z_norm) * 65535.0f);
    ddr_word_temp = (ddr_word_t)internal_val_z;
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_z = ps_float_to_fp_z_buffer((1.0f - tri_ps->v1z_norm) * 65535.0f);
    ddr_word_temp = (ddr_word_t)internal_val_z;
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_z = ps_float_to_fp_z_buffer((1.0f - tri_ps->v2z_norm) * 65535.0f);
    ddr_word_temp = (ddr_word_t)internal_val_z;
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // Edge equations
    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge0_A);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge0_B);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge0_C);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge1_A);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge1_B);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge1_C);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge2_A);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge2_B);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->edge2_C);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // Depth gradients
    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->dzdx);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_coord = ps_float_to_fp_coord_internal(tri_ps->dzdy);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_coord);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    // Color (R, G, B)
    internal_val_color = ps_to_fp_color_channel((float)tri_ps->r);
    ddr_word_temp = (ddr_word_t)internal_val_color;
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_color = ps_to_fp_color_channel((float)tri_ps->g);
    ddr_word_temp = (ddr_word_t)internal_val_color;
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    internal_val_color = ps_to_fp_color_channel((float)tri_ps->b);
    ddr_word_temp = (ddr_word_t)((s64)internal_val_color);
    output_ddr_words[current_packet_word_offset + word_idx++] = ddr_word_temp;

    if (word_idx != NUM_AXI_WORDS_PER_PACKET) {
        xil_printf("ERROR: Mismatch in expected words per packet! Expected %u, but wrote %u for triangle %u\r\n", NUM_AXI_WORDS_PER_PACKET, word_idx, triangle_idx);
        // An assert or exit might be appropriate here in a production system.
    }
}

/*-----------------------------------------------------------*/
// 定义任务句柄
static TaskHandle_t xHdmiInitTaskHandle = NULL;
static TaskHandle_t xGpuRenderTaskHandle = NULL;

// 定义FreeRTOS队列，用于任务间通信，例如HDMI初始化完成后通知显示任务
static QueueHandle_t xHdmiReadyQueue = NULL; // 用于通知初始化完成

// 动画变量
static float animation_offset_x = 0.0f;
static float animation_offset_y = 0.0f;
static float animation_offset_speed_x = 1.0f; // 像素/帧
static float animation_offset_speed_y = 0.5f; // 像素/帧
static u32 frame_count = 0;


/*-----------------------------------------------------------*/
// FreeRTOS 任务函数原型
static void vHdmiInitTask( void *pvParameters );
static void vGpuRenderTask( void *pvParameters );

/*-----------------------------------------------------------*/
// FreeRTOS 静态内存分配 (如果 configSUPPORT_STATIC_ALLOCATION == 1)
#if (configSUPPORT_STATIC_ALLOCATION == 1)
// 为 HDMI 初始化任务定义静态内存
StackType_t xHdmiInitStack[ configMINIMAL_STACK_SIZE * 4 ];
StaticTask_t xHdmiInitBuffer;

// 为 GPU 渲染任务定义静态内存 (需要更多栈空间，因为它处理浮点计算和大量数据)
// 增加栈大小，特别是为了调试打印
StackType_t xGpuRenderStack[ configMINIMAL_STACK_SIZE * 60 ];
StaticTask_t xGpuRenderBuffer;

// 为 HDMI 就绪队列定义静态内存 (用于传递一个 int 信号)
uint8_t ucHdmiReadyQueueStorage[ sizeof( int ) * 2 ]; // 队列深度增加到2，确保两个任务都能收到
StaticQueue_t xStaticHdmiReadyQueue;
#endif


int main( void )
{
	xil_printf( "\r\n--- FreeRTOS HDMI Output Demo(By SDJU PanZiyu) ---\r\n" );
	xil_printf( "  Compiled on: %s at %s\r\n", __DATE__, __TIME__);

	xil_printf( "  VDMA Output Resolution: %u x %u\r\n", VdmaDisplay_getOutputWidth(&g_vdma_display), VdmaDisplay_getOutputHeight(&g_vdma_display));
    xil_printf( "  VDMA Expected Bytes per Pixel: %u (BGR)\r\n", VdmaDisplay_getBytesPerPixel(&g_vdma_display));
    xil_printf( "  HLS IP Writes Bytes per Pixel: %u (BGRX)\r\n", VdmaDisplay_getHlsBytesPerPixel(&g_vdma_display));
    xil_printf( "  Using Triple Buffering with %u frames.\r\n", VDMA_NUM_FRAME_BUFFERS);
    xil_printf( "  GPU Params Buffer Base Address: 0x%x\r\n", (unsigned int)GPU_PARAMS_BUFFER_BASE_ADDR);
    xil_printf( "  GPU ZBuffer Base Address: 0x%x\r\n", (unsigned int)GPU_ZBUFFER_BASE_ADDR);

    // Initial VdmaDisplay state is printed as part of initialization (if VDMA_DISPLAY_VERBOSE_INIT is defined)

    // 创建用于任务间通信的队列 (队列深度为 2，传递一个 int，确保两个任务都能收到)
#if ( configSUPPORT_STATIC_ALLOCATION == 0 )
    xHdmiReadyQueue = xQueueCreate( 2, sizeof( int ) );
#else
    xHdmiReadyQueue = xQueueCreateStatic( 2, sizeof( int ), ucHdmiReadyQueueStorage, &xStaticHdmiReadyQueue);
#endif
    configASSERT( xHdmiReadyQueue != NULL );

    // 统一任务创建代码
#if ( configSUPPORT_STATIC_ALLOCATION == 0 )
    // 动态创建任务

    xTaskCreate( vHdmiInitTask, "HDMI Init", configMINIMAL_STACK_SIZE * 4, NULL, tskIDLE_PRIORITY + 3, &xHdmiInitTaskHandle );
    xTaskCreate( vGpuRenderTask, "GPU Render", configMINIMAL_STACK_SIZE * 60, NULL, tskIDLE_PRIORITY + 2, &xGpuRenderTaskHandle ); // 增大栈

#else
    xil_printf( "  Using static memory for FreeRTOS tasks and queues.\r\n" );
    // 静态创建任务
    xHdmiInitTaskHandle = xTaskCreateStatic( vHdmiInitTask, "HDMI Init", configMINIMAL_STACK_SIZE * 4, NULL, tskIDLE_PRIORITY + 3, xHdmiInitStack, &xHdmiInitBuffer );
    configASSERT( xHdmiInitTaskHandle != NULL );

    xGpuRenderTaskHandle = xTaskCreateStatic( vGpuRenderTask, "GPU Render", configMINIMAL_STACK_SIZE * 60, NULL, tskIDLE_PRIORITY + 2, xGpuRenderStack, &xGpuRenderBuffer ); // 增大栈
    configASSERT( xGpuRenderTaskHandle != NULL );
#endif

    // 启动 FreeRTOS 调度器
    vTaskStartScheduler();

    // 如果调度器启动失败（例如，内存不足），程序将执行到这里
    xil_printf( "Error: Scheduler failed to start or tasks terminated unexpectedly.\r\r\n" );
    for( ;; ); // main 函数不应该返回
}

// HDMI 初始化任务实现：负责 VDMA 和 HDMI 链路的一次性设置
static void vHdmiInitTask( void *pvParameters )
{
    int received_signal = 0;
    ( void ) pvParameters; // 消除未使用的参数警告

    xil_printf("\r\n[HDMI Init Task]: Starting VDMA initialization.\r\n");

    // 调用封装的 VDMA 初始化函数
    VdmaDisplay_init_struct(&g_vdma_display); // 首先初始化结构体成员，包括信号量句柄
    int status = VdmaDisplay_init(&g_vdma_display); // 然后创建信号量并初始化VDMA

    if (status != XST_SUCCESS) {
        xil_printf("[HDMI Init Task]: FATAL ERROR: VDMA initialization failed! Status=%d.\r\n", status);
        xil_printf("                 System in degraded state. HDMI output will not function.\r\n");
        for( ;; ) {
            vTaskDelay( portMAX_DELAY ); // 永久阻塞，不消耗CPU
        }
    }

    // 初始化 GPU_Render_TBR IP
    XGpu_render_tbr_Config *GpuRenderTbrCfgPtr;

    GpuRenderTbrCfgPtr = XGpu_render_tbr_LookupConfig(GPU_RENDER_TBR_BASE_ADDR);
    if (GpuRenderTbrCfgPtr == NULL) {
        xil_printf("[HDMI Init Task]: FATAL ERROR: XGpu_render_tbr_LookupConfig failed! Check Xparameters.h or Block Design. BaseAddress=0x%x\r\n", (unsigned int)GPU_RENDER_TBR_BASE_ADDR);
        for( ;; ) { vTaskDelay( portMAX_DELAY ); }
    }

    status = XGpu_render_tbr_CfgInitialize(&GpuRenderTbrInstance, GpuRenderTbrCfgPtr);
    if (status != XST_SUCCESS) {
        xil_printf("[HDMI Init Task]: FATAL ERROR: XGpu_render_tbr_CfgInitialize failed! Status=%d.\r\n", status);
        for( ;; ) { vTaskDelay( portMAX_DELAY ); }
    }
    xil_printf("[HDMI Init Task]: GPU_Render_TBR IP initialized successfully.\r\n");

    // 自检：检查 GPU IP 是否处于就绪状态
    if (!XGpu_render_tbr_IsReady(&GpuRenderTbrInstance)) {
        xil_printf("[HDMI Init Task]: WARNING: GPU_Render_TBR IP is not ready after initialization.\r\r\n");
    } else {
        xil_printf("[HDMI Init Task]: GPU_Render_TBR IP is ready.\r\n");
    }

    // VDMA 和 GPU IP 初始化成功后，发送信号通知其他任务
    // 因为队列深度是2，所以发送两次，确保两个任务都能收到
    if (xQueueSend( xHdmiReadyQueue, &received_signal, portMAX_DELAY ) != pdPASS) { // Use received_signal (value 0) or any constant int.
        xil_printf("[HDMI Init Task]: ERROR: Failed to send HDMI ready signal (1) to queue.\r\n");
    }
    // 第二次发送，确保 GPU Render Task 可以接收到
    if (xQueueSend( xHdmiReadyQueue, &received_signal, portMAX_DELAY ) != pdPASS) { // Use received_signal (value 0) or any constant int.
        xil_printf("[HDMI Init Task]: ERROR: Failed to send HDMI ready signal (2) to queue.\r\n");
    } else {
        xil_printf("[HDMI Init Task]: Signals sent to GPU Render (and potentially other) Tasks.\r\n");
    }


    // HDMI 初始化任务完成其工作，可以安全地删除自身。
    xil_printf("[HDMI Init Task]: Initialization complete, deleting task.\r\n");
    vTaskDelete( NULL ); // 删除当前任务
}

// --- GPU 渲染任务实现 ---
static void vGpuRenderTask( void *pvParameters )
{
    int received_signal = 0;
    ( void ) pvParameters;

    // FreeRTOS Tick Rate (ticks per second)
    const float ticks_to_ms_factor = 1000.0f / (float)configTICK_RATE_HZ;

    // 等待 HDMI 初始化任务完成信号
    xil_printf("[GPU Render Task]: Waiting for HDMI Init completion signal...\r\n");
    if (xQueueReceive( xHdmiReadyQueue, &received_signal, portMAX_DELAY ) != pdPASS) {
        xil_printf("[GPU Render Task]: FATAL ERROR: Failed to receive HDMI ready signal from queue. Task cannot proceed.\r\n");
        for( ;; ) { vTaskDelay( portMAX_DELAY ); }
    }
    xil_printf("[GPU Render Task]: HDMI Init completed. Starting GPU rendering loop.\r\n");

    u32 output_width = VdmaDisplay_getOutputWidth(&g_vdma_display);
    u32 output_height = VdmaDisplay_getOutputHeight(&g_vdma_display);
    u32 hls_single_frame_size = VdmaDisplay_getHlsSingleFrameSize(&g_vdma_display); // HLS 帧缓冲区大小
    u32 vdma_single_frame_size = VdmaDisplay_getVdmaSingleFrameSize(&g_vdma_display); // VDMA 帧缓冲区大小

    // --- 准备三角形数据 ---
    PSTriangle test_triangles_ps[MAX_NUM_TRIANGLES]; // 使用 C 数组
    u32 num_actual_triangles = 0; // 记录实际添加的三角形数量

    xil_printf("[GPU Render Task]: Preparing initial test triangles...\r\n");

    // ----- 动画正方形的第一个三角形 (左上到右下角的对角线分割) -----
    // 初始位置和大小
    float tri1_x = 50.0f;
    float tri1_y = 50.0f;
    float tri1_size = 100.0f;
    float tri1_z_norm_base = 0.5f;
    u8 tri1_r_base = 255; u8 tri1_g_base = 0; u8 tri1_b_base = 0; // 红色

    if (num_actual_triangles < MAX_NUM_TRIANGLES) {
        PSTriangle* current_tri = &test_triangles_ps[num_actual_triangles];
        current_tri->v0x=tri1_x; current_tri->v0y=tri1_y; current_tri->v0z_norm=tri1_z_norm_base;
        current_tri->v1x=tri1_x+tri1_size; current_tri->v1y=tri1_y; current_tri->v1z_norm=tri1_z_norm_base + 0.05f;
        current_tri->v2x=tri1_x; current_tri->v2y=tri1_y+tri1_size; current_tri->v2z_norm=tri1_z_norm_base + 0.1f;
        current_tri->r=tri1_r_base; current_tri->g=tri1_g_base; current_tri->b=tri1_b_base;
        calculate_edge_equations_float(current_tri->v0x, current_tri->v0y, current_tri->v1x, current_tri->v1y, current_tri->v2x, current_tri->v2y,
            &current_tri->edge0_A, &current_tri->edge0_B, &current_tri->edge0_C,
            &current_tri->edge1_A, &current_tri->edge1_B, &current_tri->edge1_C,
            &current_tri->edge2_A, &current_tri->edge2_B, &current_tri->edge2_C);
        calculate_depth_gradients_float(current_tri->v0x, current_tri->v0y, current_tri->v0z_norm, current_tri->v1x, current_tri->v1y, current_tri->v1z_norm, current_tri->v2x, current_tri->v2y, current_tri->v2z_norm,
            &current_tri->dzdx, &current_tri->dzdy);
        num_actual_triangles++;
    }

    // --- 动画正方形的第二个三角形 ---
    if (num_actual_triangles < MAX_NUM_TRIANGLES) {
        PSTriangle* current_tri = &test_triangles_ps[num_actual_triangles];
        // 顶点定义：(v1x,v1y) of tri1, (v1x+size, v1y+size), (v2x,v2y) of tri1
        // 实际上是 (右顶点, 右下顶点, 下顶点)
        current_tri->v0x=tri1_x+tri1_size; current_tri->v0y=tri1_y; current_tri->v0z_norm=tri1_z_norm_base + 0.05f;
        current_tri->v1x=tri1_x+tri1_size; current_tri->v1y=tri1_y+tri1_size; current_tri->v1z_norm=tri1_z_norm_base + 0.15f;
        current_tri->v2x=tri1_x; current_tri->v2y=tri1_y+tri1_size; current_tri->v2z_norm=tri1_z_norm_base + 0.1f;
        current_tri->r=tri1_r_base; current_tri->g=tri1_g_base; current_tri->b=tri1_b_base;
        calculate_edge_equations_float(current_tri->v0x, current_tri->v0y, current_tri->v1x, current_tri->v1y, current_tri->v2x, current_tri->v2y,
            &current_tri->edge0_A, &current_tri->edge0_B, &current_tri->edge0_C,
            &current_tri->edge1_A, &current_tri->edge1_B, &current_tri->edge1_C,
            &current_tri->edge2_A, &current_tri->edge2_B, &current_tri->edge2_C);
        calculate_depth_gradients_float(current_tri->v0x, current_tri->v0y, current_tri->v0z_norm, current_tri->v1x, current_tri->v1y, current_tri->v1z_norm, current_tri->v2x, current_tri->v2y, current_tri->v2z_norm,
            &current_tri->dzdx, &current_tri->dzdy);
        num_actual_triangles++;
    }

    // ----- 三角形 3: 覆盖大部分屏幕的黄色三角形，Z值在中间 (作为背景) -----
    if (num_actual_triangles < MAX_NUM_TRIANGLES) {
        PSTriangle* current_tri = &test_triangles_ps[num_actual_triangles];
        current_tri->v0x=0.0f; current_tri->v0y=0.0f; current_tri->v0z_norm=0.4f;
        current_tri->v1x=(float)output_width; current_tri->v1y=0.0f; current_tri->v1z_norm=0.45f;
        current_tri->v2x=(float)output_width/2.0f; current_tri->v2y=(float)output_height; current_tri->v2z_norm=0.5f;
        current_tri->r=255; current_tri->g=255; current_tri->b=0; // Yellow
        calculate_edge_equations_float(current_tri->v0x, current_tri->v0y, current_tri->v1x, current_tri->v1y, current_tri->v2x, current_tri->v2y,
            &current_tri->edge0_A, &current_tri->edge0_B, &current_tri->edge0_C,
            &current_tri->edge1_A, &current_tri->edge1_B, &current_tri->edge1_C,
            &current_tri->edge2_A, &current_tri->edge2_B, &current_tri->edge2_C);
        calculate_depth_gradients_float(current_tri->v0x, current_tri->v0y, current_tri->v0z_norm, current_tri->v1x, current_tri->v1y, current_tri->v1z_norm, current_tri->v2x, current_tri->v2y, current_tri->v2z_norm,
            &current_tri->dzdx, &current_tri->dzdy);
        num_actual_triangles++;
    }

    xil_printf("[GPU Render Task]: Total %u triangles prepared.\r\n", num_actual_triangles);

    // --- 分配 PS 端用于打包三角形数据的 DDR 内存 ---
    ddr_word_t* params_buffer = (ddr_word_t*)GPU_PARAMS_BUFFER_BASE_ADDR;
    if (params_buffer == NULL) {
        xil_printf("[GPU Render Task]: ERROR: Failed to allocate params_buffer at 0x%x.\r\n", (unsigned int)GPU_PARAMS_BUFFER_BASE_ADDR);
        for( ;; ) { vTaskDelay( portMAX_DELAY ); }
    }
    Xil_DCacheFlushRange((INTPTR)params_buffer, num_actual_triangles * NUM_AXI_WORDS_PER_PACKET * sizeof(ddr_word_t));
    xil_printf("[GPU Render Task]: Allocated params_buffer at 0x%x.\r\n", params_buffer);

    // --- Z-buffer 分配 ---
    u16* zbuffer_raw_ptr = (u16*)GPU_ZBUFFER_BASE_ADDR;
    u32 zbuffer_size_bytes = output_width * output_height * sizeof(u16);

    if (zbuffer_raw_ptr == NULL) {
        xil_printf("[GPU Render Task]: ERROR: Failed to allocate zbuffer at 0x%x.\r\n", (unsigned int)GPU_ZBUFFER_BASE_ADDR);
        for( ;; ) { vTaskDelay( portMAX_DELAY ); }
    }

    // --- 渲染循环 ---
    const u32 FPS_MEASUREMENT_INTERVAL_FRAMES = 10; // 每10帧测量一次FPS，以获得更平滑的平均值

    TickType_t gpu_start_ticks; // 记录GPU渲染开始的FreeRTOS tick
    TickType_t fps_last_measurement_start_ticks = xTaskGetTickCount(); // 用于FPS平滑计算的起始tick
    u32 frames_since_last_measurement = 0; // 记录自上次FPS测量以来完成的帧数
    float current_fps = 0.0f; // 当前显示在屏幕上的FPS值

    // --- 调试计时变量 ---
    TickType_t loop_start_ticks; // 测量每个完整帧循环的开始

    for( ;; )
    {
        loop_start_ticks = xTaskGetTickCount(); // 记录当前帧循环开始时间

        // --- 同步：等待一个可用的渲染缓冲区 ---
        // 尝试获取信号量。如果计数为0，任务将阻塞直到有缓冲区变为可用。
        if (xSemaphoreTake(g_vdma_display.xRenderingBufferAvailableSemaphore_, portMAX_DELAY) != pdPASS) {
            xil_printf("[GPU Render Task]: FATAL ERROR: Failed to take xRenderingBufferAvailableSemaphore_.\r\n");
            for( ;; ) { vTaskDelay( portMAX_DELAY ); } // 严重错误，任务无法继续
        }
        
        // 1. 获取当前要渲染到的 HLS 帧缓冲区地址和索引
        int current_render_fb_idx = VdmaDisplay_getNextHlsWriteBufferIdx(&g_vdma_display);
        UINTPTR hls_framebuffer_addr = VdmaDisplay_getHlsFrameBufferAddr(&g_vdma_display, current_render_fb_idx);

        if (hls_framebuffer_addr == 0) {
            xil_printf("[GPU Render Task]: ERROR: Failed to get valid HLS framebuffer address for index %d. Retrying.\r\n", current_render_fb_idx);
            // 失败时释放信号量，以便其他任务可以尝试渲染
            xSemaphoreGive(g_vdma_display.xRenderingBufferAvailableSemaphore_);
            vTaskDelay( pdMS_TO_TICKS(10) );
            continue; // 跳过当前帧，等待下一轮
        }

        // 2. 清空目标 HLS 帧缓冲区为默认背景色 (灰色 BGRX: 0x00323232)
        u32 default_color_slot = ps_pack_rgb_to_32bit_slot(0x32, 0x32, 0x32); // R=50, G=50, B=50 -> 0x00323232

        u32* hls_framebuffer_ptr_u32 = (u32*)hls_framebuffer_addr;
        for (u32 i = 0; i < (output_width * output_height); ++i) {
            hls_framebuffer_ptr_u32[i] = default_color_slot; // 每个像素写入 0x00RRGGBB
        }
        Xil_DCacheFlushRange((INTPTR)hls_framebuffer_addr, hls_single_frame_size);

        // 3. 重置 ZBuffer 为最远值 (0xFFFF for each 16-bit Z value)
        // 注意：这里是对ZBuffer数据进行操作，ZBuffer的缓存刷新范围是zbuffer_size_bytes
        Xil_DCacheInvalidateRange((INTPTR)zbuffer_raw_ptr, zbuffer_size_bytes); // 确保 CPU 读取 ZBuffer 为最新状态
        for (u32 i = 0; i < (output_width * output_height); ++i) {
            zbuffer_raw_ptr[i] = 0xFFFF;
        }
        Xil_DCacheFlushRange((INTPTR)zbuffer_raw_ptr, zbuffer_size_bytes); // 清零后 Flush 回 DDR

        // --- 更新动画数据 ---
        // 更新动画偏移量
        animation_offset_x += animation_offset_speed_x;
        animation_offset_y += animation_offset_speed_y;

        // X轴边界检查
        if (animation_offset_x + tri1_x + tri1_size > (float)output_width || animation_offset_x + tri1_x < 0.0f) {
            animation_offset_speed_x *= -1.0f; // 反转方向
            animation_offset_x += animation_offset_speed_x; // 稍微修正位置以避免卡在边界
        }
        // Y轴边界检查
        if (animation_offset_y + tri1_y + tri1_size > (float)output_height || animation_offset_y + tri1_y < 0.0f) {
            animation_offset_speed_y *= -1.0f; // 反转方向
            animation_offset_y += animation_offset_speed_y; // 稍微修正位置以避免卡在边界
        }

        // 更新动画正方形的第一个三角形 (test_triangles_ps[0])
        PSTriangle* animated_tri1 = &test_triangles_ps[0];
        animated_tri1->v0x = tri1_x + animation_offset_x;
        animated_tri1->v0y = tri1_y + animation_offset_y;
        animated_tri1->v1x = tri1_x + tri1_size + animation_offset_x;
        animated_tri1->v1y = tri1_y + animation_offset_y;
        animated_tri1->v2x = tri1_x + animation_offset_x;
        animated_tri1->v2y = tri1_y + tri1_size + animation_offset_y;

        // 更新动画正方形的第二个三角形 (test_triangles_ps[1])
        PSTriangle* animated_tri2 = &test_triangles_ps[1];
        animated_tri2->v0x = tri1_x + tri1_size + animation_offset_x;
        animated_tri2->v0y = tri1_y + animation_offset_y;
        animated_tri2->v1x = tri1_x + tri1_size + animation_offset_x;
        animated_tri2->v1y = tri1_y + tri1_size + animation_offset_y;
        animated_tri2->v2x = tri1_x + animation_offset_x;
        animated_tri2->v2y = tri1_y + tri1_size + animation_offset_y;

        // 颜色动画 (两个三角形使用相同的颜色)
        u8 animated_b_color = (u8)(((frame_count * 5) % 256));
        u8 animated_g_color = (u8)((int)(animation_offset_x * 0.5f) % 256);
        // R分量保持不变 (255)

        animated_tri1->b = animated_b_color;
        animated_tri1->g = animated_g_color;
        animated_tri2->b = animated_b_color;
        animated_tri2->g = animated_g_color;

        // 重新计算动画三角形的边缘方程和深度梯度 (对两个动画三角形都做)
        calculate_edge_equations_float(animated_tri1->v0x, animated_tri1->v0y, animated_tri1->v1x, animated_tri1->v1y, animated_tri1->v2x, animated_tri1->v2y,
            &animated_tri1->edge0_A, &animated_tri1->edge0_B, &animated_tri1->edge0_C,
            &animated_tri1->edge1_A, &animated_tri1->edge1_B, &animated_tri1->edge1_C,
            &animated_tri1->edge2_A, &animated_tri1->edge2_B, &animated_tri1->edge2_C);
        calculate_depth_gradients_float(animated_tri1->v0x, animated_tri1->v0y, animated_tri1->v0z_norm, animated_tri1->v1x, animated_tri1->v1y, animated_tri1->v1z_norm, animated_tri1->v2x, animated_tri1->v2y, animated_tri1->v2z_norm,
            &animated_tri1->dzdx, &animated_tri1->dzdy);

        calculate_edge_equations_float(animated_tri2->v0x, animated_tri2->v0y, animated_tri2->v1x, animated_tri2->v1y, animated_tri2->v2x, animated_tri2->v2y,
            &animated_tri2->edge0_A, &animated_tri2->edge0_B, &animated_tri2->edge0_C,
            &animated_tri2->edge1_A, &animated_tri2->edge1_B, &animated_tri2->edge1_C,
            &animated_tri2->edge2_A, &animated_tri2->edge2_B, &animated_tri2->edge2_C);
        calculate_depth_gradients_float(animated_tri2->v0x, animated_tri2->v0y, animated_tri2->v0z_norm, animated_tri2->v1x, animated_tri2->v1y, animated_tri2->v1z_norm, animated_tri2->v2x, animated_tri2->v2y, animated_tri2->v2z_norm,
            &animated_tri2->dzdx, &animated_tri2->dzdy);


        // 4. 将三角形参数打包并写入 params_buffer
        for (u32 i = 0; i < num_actual_triangles; ++i) {
            pack_raster_params_to_ddr(&test_triangles_ps[i], params_buffer, i);
        }
        Xil_DCacheFlushRange((INTPTR)params_buffer, num_actual_triangles * NUM_AXI_WORDS_PER_PACKET * sizeof(ddr_word_t));

        // 5. 配置并启动 GPU_Render_TBR IP
        XGpu_render_tbr_Set_params_buffer_base_addr(&GpuRenderTbrInstance, (u64)GPU_PARAMS_BUFFER_BASE_ADDR);
        XGpu_render_tbr_Set_num_triangles_to_process(&GpuRenderTbrInstance, num_actual_triangles);
        XGpu_render_tbr_Set_max_x_res(&GpuRenderTbrInstance, output_width);
        XGpu_render_tbr_Set_max_y_res(&GpuRenderTbrInstance, output_height);
        XGpu_render_tbr_Set_zbuffer_ddr_raw_ptr(&GpuRenderTbrInstance, (u64)GPU_ZBUFFER_BASE_ADDR);
        XGpu_render_tbr_Set_framebuffer_ddr_raw_ptr(&GpuRenderTbrInstance, (u64)hls_framebuffer_addr);

        gpu_start_ticks = xTaskGetTickCount(); // <<< 在HLS IP启动前记录时间
        XGpu_render_tbr_Start(&GpuRenderTbrInstance);

        // 6. 等待 GPU_Render_TBR IP 完成
        while (!XGpu_render_tbr_IsDone(&GpuRenderTbrInstance)) {
            vTaskDelay(pdMS_TO_TICKS(1)); // 短暂延时，避免忙等
        }
        TickType_t gpu_end_ticks = xTaskGetTickCount(); // <<< 在HLS IP完成后记录时间

        // --- 在 GPU 渲染完成后，确保 CPU 缓存失效，以便后续的像素转换能获取到由 HLS IP 写入的最新像素数据 ---
        Xil_DCacheInvalidateRange((INTPTR)hls_framebuffer_addr, hls_single_frame_size);

        // ====================================================================================
        // HLS IP (4Bpp BGRX) -> PS 转换 (3Bpp BGR)
        // ====================================================================================
        UINTPTR vdma_framebuffer_addr = VdmaDisplay_getVdmaFrameBufferAddr(&g_vdma_display, current_render_fb_idx);

        u32* src_ptr_u32 = (u32*)hls_framebuffer_addr; // HLS 写入的 4Bpp BGRX
        u8* dest_ptr_u8 = (u8*)vdma_framebuffer_addr;  // VDMA 期望的 3Bpp BGR

        if (vdma_framebuffer_addr == 0) {
            xil_printf("[GPU Render Task]: ERROR: Failed to get valid VDMA framebuffer address for index %d. Conversion aborted.\r\n", current_render_fb_idx);
            // 如果转换失败，也需要释放信号量，否则会导致死锁。
            xSemaphoreGive(g_vdma_display.xRenderingBufferAvailableSemaphore_);
        } else {
            for (u32 i = 0; i < (output_width * output_height); ++i) {
                u32 pixel_bgrx = src_ptr_u32[i]; // 读取一个 32 位的 BGRX 像素 (0x00RRGGBB)

                // 提取 B, G, R 分量
                u8 b = (u8)(pixel_bgrx & 0xFF);         // Blue
                u8 g = (u8)((pixel_bgrx >> 8) & 0xFF);  // Green
                u8 r = (u8)((pixel_bgrx >> 16) & 0xFF); // Red

                // 写入 3 字节 BGR 到 VDMA 目标缓冲区
                dest_ptr_u8[i * 3 + 0] = b;
                dest_ptr_u8[i * 3 + 1] = g;
                dest_ptr_u8[i * 3 + 2] = r;
            }

            // <<< 绘制帧率文本 >>>
            // 测量 HLS IP 渲染时间，并每 FPS_MEASUREMENT_INTERVAL_FRAMES 帧更新一次屏幕上的FPS显示
            frames_since_last_measurement++;
            if (frames_since_last_measurement >= FPS_MEASUREMENT_INTERVAL_FRAMES) {
                TickType_t elapsed_ticks = gpu_end_ticks - fps_last_measurement_start_ticks; // 使用整个测量周期的总时间

                if (elapsed_ticks == 0) {
                    current_fps = (float)configTICK_RATE_HZ; // 假定帧时间小于一个 tick
                } else {
                    float total_elapsed_ms = (float)elapsed_ticks * ticks_to_ms_factor;
                    current_fps = (float)frames_since_last_measurement * 1000.0f / total_elapsed_ms;
                }

                // 重置计数器和起始时间
                fps_last_measurement_start_ticks = gpu_end_ticks;
                frames_since_last_measurement = 0;
            }

            // 绘制 FPS 值，使用红色字体 (R=255, G=0, B=0)
            DrawString(dest_ptr_u8, output_width, output_height, 10, 10, "FPS:", 255, 0, 0); // 红色文字 "FPS:"
            DrawNumber(dest_ptr_u8, output_width, output_height, 10 + FONT_WIDTH * 4, 10, (int)my_roundf(current_fps), 255, 0, 0); // 红色数字

            // 刷新 VDMA 目标缓冲区缓存，确保转换后的数据写入 DDR
            Xil_DCacheFlushRange((INTPTR)vdma_framebuffer_addr, vdma_single_frame_size);
        }

        // 8. 通知 VDMA 显示 PS 刚刚转换完成的帧缓冲区
        // 此函数内部会释放 xRenderingBufferAvailableSemaphore_ 信号量
        int status_vdma = VdmaDisplay_commitSpecificBuffer(&g_vdma_display, current_render_fb_idx);
        if (status_vdma != XST_SUCCESS) {
            xil_printf("[GPU Render Task]: ERROR: Failed to commit VDMA-ready buffer %d for display! Status=%d\r\n", current_render_fb_idx, status_vdma);
            // 注意：如果 commitSpecificBuffer 失败，它应该已经尝试释放信号量。
            // 如果没有，这里需要再次确保信号量被释放，避免死锁。
            // 但在当前的实现中，commitSpecificBuffer 在失败时也会尝试释放互斥量，并在逻辑上释放渲染信号量。
        }

        frame_count++;
    }
}

